#include <memory>
#include <vector>
#include <chrono>
#include <future>
#include <queue>
// 风格: 型别名称用山峰风格, 变量名用小写加 _ 风格
// FullCommunicationStruct是主要载体，里面加 SptStructData
namespace hy
{
    // volatile 读写操作不可以被优化 atd::atomic 原子操作变量
    template <typename T>   // 通用指针模板
    using Sptr = std::shared_ptr<T>;

    using VectorInt = std::vector<int>;
    using VectorDouble = std::vector<double>;
    using AtomicBool = volatile std::atomic<bool>;       // 用于flag，读写操作不可被优化
    using SptrFlag = std::shared_ptr<std::atomic<bool>>;    // 共享指针指向的原子变量bool，在这个体系中当标志位
    using VectorAtomicDouble = std::vector<std::atomic<double>>;
    using HighResolutionTime = std::chrono::_V2::system_clock::time_point;  //= std::chrono::high_resolution_clock::now(); //计时用
    struct ImuData {
        std::atomic<int> status;    // 状态，可能会用到
        std::atomic<int> flag;     // 是否可用,因为其中的时间不是原子的(这个data可不可以用)
        VectorAtomicDouble data;    // imu_data: vector(6)
        HighResolutionTime time;// = std::chrono::high_resolution_clock::now(); //计时用 // 时间戳
        ImuData(): time(std::chrono::high_resolution_clock::now()),data(6) {flag.store(0);}
        // ImuData(): flag(false), time(std::chrono::high_resolution_clock::now()){} // TODO 传参初始化
        void update(std::vector<double>& imu_vec){
            for(int i=0;i<6;i++){
                data[i].store(imu_vec[i]);
            }
            flag.store(1);
        }
        void printImuData(){
            std::cout<<"ImuData: [ ";
            for(int i=0;i<6;i++){
                std::cout<<data[i].load()<<"\t";
            }
            std::cout<<"]"<<std::endl;
        }
    };
    using SptrImuData = std::shared_ptr<ImuData>;
    // 完整通信结构体
    template<typename T>            // T是具体的数据结构体
    struct FullCommunicationStruct{ // 完整通信结构体，通信中两个线程通过这一个(仅一个)结构体沟通某一个类型的变量(T)，
        AtomicBool flag;            // 这个通信内存块可不可以用，同一时刻只能有一个人用，有人在用就是true，无人用就是false
        std::queue<Sptr<T>>  msg_queue;             // 消息队列，里面存指向实际消息块的智能指针
        FullCommunicationStruct(){}
        // 发送和订阅都使用移动构造 shared智能指针，会比较快
        int Publish(Sptr<T>& sptr_data){
            while(flag.load() == true){std::this_thread::sleep_for(std::chrono::microseconds(50));} // 正在使用就等x微秒
            flag.store(true);   // 使用
            msg_queue.push(std::move(sptr_data));   // 用这个把新消息入队列，数据指针交由队列管理
            flag.store(false);   // 放手
            return 1;
        }
        bool IsEmpty(void){     // 判断队列是否为空
            return msg_queue.size()==0? true:false;
        }
        Sptr<T> Subscribe(){
            while(1){
                while(flag.load() == true){std::this_thread::sleep_for(std::chrono::microseconds(100));} // 正在使用就等x微秒
                flag.store(true);   // 使用
                if (msg_queue.size() != 0){ // 非空就订阅
                    Sptr<T> sptr_data(std::move(msg_queue.front()));   // 把最前面的消息取出来
                    msg_queue.pop();    // 取一个消息就弹出一个消息，析构数据操作由智能指针完成
                    flag.store(false);   // 放手
                    return sptr_data;
                }
                else{   // 空的，那就放手，等着
                    flag.store(false);   // 放手
                    std::this_thread::sleep_for(std::chrono::microseconds(100));
                    // std::this_thread::sleep_for(std::chrono::milliseconds(100)); // std::cout<<"waiting"<<std::endl;
                    continue;
                }
            }
        }
    };
    // 为模板本身取别名
    template <typename T>
    using FCS = FullCommunicationStruct<T>;
    // 
    template <typename T>   // 用这个可以直接表示T类型数据的完整通信数据结构体 的指针
    using SptrFCS = std::shared_ptr<FCS<T>>;

    // 工厂函数，返回指向 完整结构体<T> 的智能指针，返回值型别不要用auto，自己定义成shared类型
    template <typename T> // T 是实际的数据结构
    std::unique_ptr<hy::FCS<T>> makeSptrFCS(){
        return std::make_unique<hy::FCS<T>>();
    }

    //通信组，每一个算法共享这一个通信组里面的信息
    struct CommuTeamImuImage{
        SptrFCS<ImuData> spFC_imu;
        //  todo
        // img(两张)
        CommuTeamImuImage():spFC_imu(makeSptrFCS<ImuData>()){}
    };
    
    class VINS{
    public:
        VINS():team0(){}
        CommuTeamImuImage team0;    // 用的时候把通信组里面的指针传给相应的函数即可
        // team0 纯惯导 team1 视觉加惯导
        // 发送imu信息
        int Pub_Imu(const CommuTeamImuImage& team){
            auto sp_fcs_imu_pub(team.spFC_imu);
            std::cout << "imu sp use_count: " << sp_fcs_imu_pub.use_count() << std::endl;
            int c = 0;
            while(1){
                Sptr<ImuData> sp_imu(std::make_shared<ImuData>());
                usleep(5000);   //microseconds 微秒
                for(int i=0;i<6;i++){
                    sp_imu->data[i].store(i+c);
                }
                c++;
                sp_fcs_imu_pub->Publish(sp_imu);
            }
        }
        // 发送图片信息

        // 解算 sub
        int Cal_Imu(const CommuTeamImuImage& team){
            auto sp_fcs_imu_sub(team.spFC_imu);
            std::cout << "imu sp use_count: " << sp_fcs_imu_sub.use_count() << std::endl;
            Sptr<ImuData> last_sp_imu;
            Sptr<ImuData> sp_imu;
            int i =0;
            while(1){
                sp_imu = sp_fcs_imu_sub->Subscribe();// 这里是移动构造，快
                long long a = 0;
                for(int i=0;i<6;i++){
                    a+=sp_imu->data[i];
                }
                std::cout << "a: " << a << std::endl;

                std::cout<<"imu_data: [ ";
                for(int i=0;i<6;i++){
                    std::cout<<sp_imu->data[i]<<" ";
                }
                std::cout<<" ]\n";
                if (i != 0) {
                    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(sp_imu->time - last_sp_imu->time);
                    std::cout << "耗时: " << duration.count() << " 微秒" << std::endl;
                }
                last_sp_imu= std::move(sp_imu);
                i++;
            }
            
        }

        int Launch_Imu(){
            auto fut0 = std::async(std::launch::async, [this](){        // 发送0号数据
                    return Pub_Imu(team0);      // std::launch::async，异步方式启动Pub_Imu
            });
            auto futc = std::async(std::launch::async, [this](){        //解算
                    return Cal_Imu(team0);      // std::launch::async，异步方式启动Cal_Imu
            });
            return 1;
        }
    };
    
} // namespace hy


